function [ thetaMF ] = dcm2rv( CFM )
%DCM2RV 方向余弦矩阵转换为等效旋转矢量
%
% Input Arguments
% # CFM: 3*3*p数组，M系相对于F系的方向余弦矩阵
%
% Output Arguments
% # thetaMF: p*3矩阵，由M系旋转至F系的等效旋转矢量在M系下的投影
%
% References
% #《应用导航算法工程基础》“方向余弦矩阵转换为等效旋转矢量”

angleEps = 1/180*pi; % 1°以内使用泰勒展开式计算

thetaMF = NaN(size(CFM, 3), 3);
theta = squeeze(atan2(sqrt((CFM(2, 3, :)-CFM(3, 2, :)).^2 + (CFM(1, 3, :)-CFM(3, 1, :)).^2 + (CFM(2, 1, :)-CFM(1, 2, :)).^2), ...
    (CFM(1, 1, :)+CFM(2, 2, :)+CFM(3, 3, :)-1)));
for i=1:length(theta)
    if theta(i) <= pi/2
        if theta(i) < angleEps
            f = 1 - theta(i)^2/6 + theta(i)^4/120 + theta(i)^6/5040;
        else
            f = sin(theta(i)) / theta(i);
        end
        thetaMF(i, :) = [CFM(3, 2, i)-CFM(2, 3, i), CFM(1, 3, i)-CFM(3, 1, i), CFM(2, 1, i)-CFM(1, 2, i)]/2/f;
    else
        ctheta = cos(theta(i));
        u1abs = sqrt((CFM(1, 1, i)-1)/(1-ctheta)+1);
        u2abs = sqrt((CFM(2, 2, i)-1)/(1-ctheta)+1);
        u3abs = sqrt((CFM(3, 3, i)-1)/(1-ctheta)+1);
        if (u1abs>=u2abs) && (u1abs>=u3abs)
            u1 = u1abs * sign(CFM(3, 2, i)-CFM(2, 3, i));
            u2 = (CFM(1, 2, i)+CFM(2, 1, i))/(2*u1*(1-ctheta));
            u3 = (CFM(1, 3, i)+CFM(3, 1, i))/(2*u1*(1-ctheta));
        elseif u2abs>=u3abs
            u2 = u2abs * sign(CFM(1, 3, i)-CFM(3, 1, i));
            u1 = (CFM(1, 2, i)+CFM(2, 1, i))/(2*u2*(1-ctheta));
            u3 = (CFM(2, 3, i)+CFM(3, 2, i))/(2*u2*(1-ctheta));
        else
            u3 = u3abs * sign(CFM(2, 1, i)-CFM(1, 2, i));
            u1 = (CFM(1, 3, i)+CFM(3, 1, i))/(2*u3*(1-ctheta));
            u2 = (CFM(2, 3, i)+CFM(3, 2, i))/(2*u3*(1-ctheta));
        end
        thetaMF(i, :) = theta(i)*[u1 u2 u3];
    end
end
end